/*************************************************************************************************************************
功能：5个舵机同时缓慢转动
函数原型：servo_move(float value0, float value1, float value2, float value3, float value4, unsigned long time_action_delay);
参数释义：
        value0: 4号引脚舵机（爪子）需要转动到的角度
        value1: 6号引脚舵机（手腕）需要转动到的角度
        value2: 7号引脚舵机（小臂）需要转动到的角度
        value3: 10号引脚舵机（大臂）需要转动到的角度
        value4: 11号引脚舵机（机械臂底座转台）需要转动到的角度
        time_action_delay: 机械臂5个舵机从上一次角度转动到当前需要的角度需要等待的延时时间

***************************************************************************************************************************/
void servo_move(float value0, float value1, float value2, float value3, float value4, unsigned long time_action_delay){
  float value_arguments[] = {value0, value1, value2, value3, value4};
  float value_delta[servo_num];

  //Serial.println("---------------------------------------------------------");
  for(int i=0;i<servo_num;i++){
    value_delta[i] = (value_arguments[i] - value_init_float[i]) / servel_parts;
    //Serial.print(value_delta[i]);Serial.print(" ");
  }
  //Serial.println();

  for(int i=0;i<servel_parts;i++){
    for(int k=0;k<servo_num;k++){
      value_init_float[k] = value_delta[k] == 0 ? value_arguments[k] : value_init_float[k] + value_delta[k];
      value_init_int[k] = int(value_init_float[k]);
      //Serial.print(value_init_int[k]);Serial.print(" ");
    }
    //Serial.println();

    servo.move(value_init_int[0], value_init_int[1], value_init_int[2], value_init_int[3], value_init_int[4]);
    delay(servo_step_delay);
  }
  delay(time_action_delay);
}


//机械臂抓取物体后，将物体放置到机械臂左侧
void put_left(){
  servo_move(11, 100, 10, 82, 120, servo_action_delay_quickly);  //初始化动作，实时检测颜色的动作
  servo_move(11, 74, 10, 41, 122, servo_action_delay_slowly);    //准备抓取物体
  servo_move(90, 74, 10, 41, 122, servo_action_delay_quickly);   //爪子闭合，抓取待测物体
  servo_move(90, 100, 10, 82, 120, servo_action_delay_slowly);   //回复到初始位置
  servo_move(90, 69, 5, 39, 164, servo_action_delay_slowly);     //准备将物体放到左边
  servo_move(11, 69, 5, 39, 164, servo_action_delay_quickly);    //爪子张开，将物体放到左边
  servo_move(11, 100, 10, 82, 120, servo_action_delay_quickly);  //初始化动作，实时检测颜色的动作
}

//机械臂抓取到物体后，将物体放置到机械臂右侧
void put_right(){
  servo_move(11, 100, 10, 82, 120, servo_action_delay_quickly);  //初始化动作，实时检测颜色的动作
  servo_move(11, 74, 10, 41, 122, servo_action_delay_slowly);    //准备抓取物体
  servo_move(90, 74, 10, 41, 122, servo_action_delay_quickly);   //爪子闭合，抓取待测物体
  servo_move(90, 100, 10, 82, 120, servo_action_delay_slowly);   //回复到初始位置
  servo_move(90, 64, 5, 38, 93, servo_action_delay_slowly);      //准备将物体放到右边
  servo_move(11, 64, 5, 38, 93, servo_action_delay_quickly);     //爪子张开，将物体放到右边
  servo_move(11, 100, 10, 82, 120, servo_action_delay_quickly);  //初始化动作，实时检测颜色的动作
}
